#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf/transform_broadcaster.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>

//#include<cv_bridge/cv_bridge.h>
#include "cv_bridge/cv_bridge.h"
#include<opencv2/opencv.hpp>
#include <nav_msgs/Odometry.h>

//#include <pcl/conversions.h>

#include <map>

#include <pcl/filters/crop_box.h>

//apriltag
#include<apriltag/tag36h11.h>
#include<apriltag/apriltag.h>
#include<apriltag/common/homography.h>
#include "livox_ros_driver/CustomMsg.h"
#include<deque>
#include<thread>
#include<mutex>

using namespace Eigen;
using namespace std;
using PointType = pcl::PointXYZI;
std::map<std::string, bool> frames_;
std::vector<std::string> frames_v_;
tf2_ros::Buffer tfbuf_;
std::map<std::string, std::string> frame_ids_;
//std::vector<std_msgs::Header> pc_accum_header_;
pcl::PointCloud<PointType>::Ptr pc_local_accum_;
int frame_num_=100000;
string frame_id_string_,pc_path_name_;

Eigen::Matrix4f P2_;

cv::Mat img_,img_gray_;
apriltag_family_t *tf_ = NULL;
apriltag_detector_t *td_ = NULL;
Eigen::Matrix4d T_camera_livox_;
double tag_size_ = 0.2365;
double camera_matrix_array_[9] = {612.784362792969,0.0,323.529235839844,
                                0.0,613.156555175781,242.369125366211,
                                0.0,0.0,1.0};
cv::Mat	camera_matrix_(3,3,CV_64F,(double*)camera_matrix_array_);

ros::Publisher tag0_odom_pub_,tag2_odom_pub_;
ros::Publisher cloud_pub_;

deque<pair<double,pcl::PointCloud<PointType>::Ptr>> pc_deq_;
deque<pair<double,cv::Mat>> img_deq_;
mutex pc_mutex_,img_mutex_;
double time_last_ =0;
double time_interval_= 1;

void addObjectPoints (
    double s, cv::Matx44d T_oi, std::vector<cv::Point3d >& objectPoints)
{
  // Add to object point vector the tag corner coordinates in the bundle frame
  // Going counterclockwise starting from the bottom left corner
  objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d(-s,-s, 0, 1));
  objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d( s,-s, 0, 1));
  objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d( s, s, 0, 1));
  objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d(-s, s, 0, 1));
}

Eigen::Matrix4d getRelativeTransform(
    std::vector<cv::Point3d > objectPoints,
    std::vector<cv::Point2d > imagePoints,
    double fx, double fy, double cx, double cy)
{
  // perform Perspective-n-Point camera pose estimation using the
  // above 3D-2D point correspondences
  cv::Mat rvec, tvec;
  cv::Matx33d cameraMatrix(fx,  0, cx,
                           0,  fy, cy,
                           0,   0,  1);
  cv::Vec4f distCoeffs(0,0,0,0); // distortion coefficients
  // TODO Perhaps something like SOLVEPNP_EPNP would be faster? Would
  // need to first check WHAT is a bottleneck in this code, and only
  // do this if PnP solution is the bottleneck.
  cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
  cv::Matx33d R;
  cv::Rodrigues(rvec, R);
  Eigen::Matrix3d wRo;
  wRo << R(0,0), R(0,1), R(0,2), R(1,0), R(1,1), R(1,2), R(2,0), R(2,1), R(2,2);

  Eigen::Matrix4d T; // homogeneous transformation matrix
  T.topLeftCorner(3, 3) = wRo;
  T.col(3).head(3) <<
      tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);
  T.row(3) << 0,0,0,1;
  return T;
}

void addImagePoints (
    apriltag_detection_t *detection,
    std::vector<cv::Point2d >& imagePoints)
{
  // Add to image point vector the tag corners in the image frame
  // Going counterclockwise starting from the bottom left corner
  double tag_x[4] = {-1,1,1,-1};
  double tag_y[4] = {1,1,-1,-1}; // Negated because AprilTag tag local
                                 // frame has y-axis pointing DOWN
                                 // while we use the tag local frame
                                 // with y-axis pointing UP
  for (int i=0; i<4; i++)
  {
    // Homography projection taking tag local frame coordinates to image pixels
    double im_x, im_y;
    homography_project(detection->H, tag_x[i], tag_y[i], &im_x, &im_y);
    imagePoints.push_back(cv::Point2d(im_x, im_y));
  }
}


// void pcd2bin(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, string& out_file)
// {
//     //Create & write .bin file
// //    ofstream bin_file(out_file.c_str(),ios::out|ios::binary|ios::app);
//     ofstream bin_file(out_file.c_str());
//     if(!bin_file.good()) cout<<"Couldn't open "<<out_file<<endl;

//     //PCD 2 BIN
//     cout << "Converting "<<endl;
//     for (size_t i = 0; i < cloud->points.size (); ++i)
//     {
//         bin_file.write((char*)&cloud->points[i].x,3*sizeof(float));
//         bin_file.write((char*)&cloud->points[i].intensity,sizeof(float));
//         //cout<< 	cloud->points[i]<<endl;
//     }

//     bin_file.close();
// }

// void saveLabel(string& out_file, vector<vector<double>> label)
// {
//     ofstream outfile;      //终端输入--》内存--》文本
//     outfile.open(out_file.c_str());//(输入流) （变量）（输出文件流）
//     if(!outfile) cout<<"error"<<endl;         

//     int object_num=label.size();
//     vector<int> bbox_image;
//     for(int i=0; i<object_num; i++){

//         bbox_image=project2image2dbbox(label[i][0],label[i][1],label[i][2],label[i][3],label[i][4],label[i][5],label[i][6]);

//         outfile<<"Car "<<0<<" "<<0<<" "<<0<<" "
//         <<bbox_image[0]<<" "<<bbox_image[1]<<" "<<bbox_image[2]<<" "<<bbox_image[3]<<" "  //2dbbbox
//         <<label[i][0]<<" "<<label[i][1]<<" "<<label[i][2]<<" " //hwl
//         <<label[i][3]<<" "<<label[i][4]<<" "<<label[i][5]<<" " //xyz
//         <<label[i][6]<<" "<<0<<std::endl;  //yaw
//     }
//     outfile.close();
// }

// void saveImgLabel(string& out_file, vector<vector<double>> label, string img_path, cv::Mat img,
//     Eigen::Matrix4f T_cam_livox, Eigen::Matrix4f T_livox_plane, Eigen::Matrix4f T_plane_c_plane)
// {
//     ofstream outfile;      //终端输入--》内存--》文本
//     outfile.open(out_file.c_str(), ios::out|ios::binary|ios::app);//(输入流) （变量）（输出文件流）
//     if(!outfile) cout<<"error"<<endl;         
//     outfile<<img_path<<" ";

//     int object_num=label.size();
//     vector<int> bbox_image;
//     int center_x, center_y;
//     for(int i=0; i<object_num; i++){
//         {
//             Eigen::Vector3f pt;
//             pt = (P2_*T_cam_livox*T_livox_plane*T_plane_c_plane*
//                         Eigen::Vector4f(label[i][3], label[i][4] - label[i][0]/2.0, label[i][5], 1)).topRows(3);
//             int32_t u = static_cast<int32_t>(pt(0)/pt(2));
//             int32_t v = static_cast<int32_t>(pt(1)/pt(2));
//             // cout<<"center x y "<<u<<" "<<v<<endl;
//             if (u>=0 && u<= 640 && v >= 0 && v <= 480 ){
//                 center_x = u;
//                 center_y = v;
//             }else{
//                 continue;
//             }
//         }
//         if(i == 0)
//             circle(img, cv::Point(center_x, center_y), 2, cv::Scalar(255,0, 0), 4);
//         if(i == 1)
//             circle(img, cv::Point(center_x, center_y), 2, cv::Scalar(0,255, 0), 4);
//         if(i == 2)
//             circle(img, cv::Point(center_x, center_y), 2, cv::Scalar(255,255, 0), 4);

//         outfile<<center_x<<","<<center_y<<","<<i<<" ";
//     }
//     outfile<<endl;
//     outfile.close();
//     cv::imshow("img_center", img);
//     cv::waitKey(1);
// }

void changeLivoxCustom2PC2(const livox_ros_driver::CustomMsg::ConstPtr &msg,pcl::PointCloud<PointType>::Ptr p_pc){
    pcl::PointCloud<PointType>::Ptr p_pc_raw(new pcl::PointCloud<PointType>);
    p_pc_raw->points.resize(msg->point_num);

    for (uint i = 0; i < msg->point_num; i++)
    {
        if ((msg->points[i].line < 6) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00))
        {
            p_pc_raw->points[i].x = msg->points[i].x;
            p_pc_raw->points[i].y = msg->points[i].y;
            p_pc_raw->points[i].z = msg->points[i].z;
            p_pc_raw->points[i].intensity = msg->points[i].reflectivity;
            p_pc->points.push_back(p_pc_raw->points[i]);
        }
    }
}

void pc_custom_CB(const livox_ros_driver::CustomMsg::Ptr& pc_msg){
    ROS_INFO("pc CB1");
    pcl::PointCloud<PointType>::Ptr pc_tmp(new pcl::PointCloud<PointType>);//转成pcl点云
    changeLivoxCustom2PC2(pc_msg,pc_tmp);
    pc_tmp->width = 1;
    pc_tmp->height = pc_tmp->size();
    lock_guard<mutex> lg(pc_mutex_);
    pc_deq_.push_back({pc_msg->header.stamp.toSec(),pc_tmp});
    pc_deq_.back().second = pc_tmp;
    ROS_INFO("pc CB2");
}

void img_CB(const sensor_msgs::CompressedImageConstPtr& front_image_msg){
    ROS_INFO("img CB1");
    cv_bridge::CvImagePtr image_front;
    image_front = cv_bridge::toCvCopy(front_image_msg, sensor_msgs::image_encodings::BGR8);
    cv::Mat img((*image_front).image);
    lock_guard<mutex> lg(img_mutex_);
    img_deq_.push_back({front_image_msg->header.stamp.toSec(),img});
    img.copyTo(img_deq_.back().second);
    ROS_INFO("img CB2");
}

void processThread(){
    while(true){
        pair<double,cv::Mat> img_pair_front,img_pair_back,img_pair;
         img_pair_front = {0,cv::Mat(480,640,CV_8UC3,cv::Scalar(0,0,0))};
         img_pair_back= {0,cv::Mat(480,640,CV_8UC3,cv::Scalar(0,0,0))};
         img_pair= {0,cv::Mat(480,640,CV_8UC3,cv::Scalar(0,0,0))};
        pair<double,pcl::PointCloud<PointType>::Ptr> pc_pair;

        {
            lock_guard<mutex> lg1(pc_mutex_);
            lock_guard<mutex> lg(img_mutex_);
            while(!pc_deq_.empty()&&pc_deq_.front().first<=img_deq_.front().first){
                pc_deq_.pop_front();
            }
            if(pc_deq_.empty()||img_deq_.empty()||pc_deq_.front().first>=img_deq_.back().first){
                continue;
            }
            pc_pair.first = pc_deq_.front().first;
            pc_pair.second = pc_deq_.front().second;
            pc_deq_.pop_front();
            while(!img_deq_.empty()&&img_deq_.front().first<=pc_pair.first){
                img_pair_front = img_deq_.front();
                img_deq_.front().second.copyTo(img_pair_front.second);
                img_deq_.pop_front();
            }
            img_pair_back = img_deq_.front();
            img_deq_.front().second.copyTo(img_pair_back.second);
            if(fabs(img_pair_front.first-pc_pair.first)<fabs(img_pair_back.first-pc_pair.first)){
                img_pair.first = img_pair_front.first;
                img_pair_front.second.copyTo( img_pair.second);
            }else{
                img_pair.first = img_pair_back.first;
                img_pair_back.second.copyTo( img_pair.second); 
            }
        }

        if( (pc_pair.first - time_last_ )< time_interval_){
            continue;
        }
        time_last_ = pc_pair.first;
        
        pcl::PointCloud<PointType>::Ptr pc_tmp(new pcl::PointCloud<PointType>);
        pc_tmp = pc_pair.second;
        img_pair.second.copyTo(img_);

        ROS_INFO("cb ok");
        frame_id_string_=(__gnu_cxx::__to_xstring<string>(&std::vsnprintf, 4 * sizeof(int),
                                                        "%06d", frame_num_));

        cv::cvtColor(img_, img_gray_, CV_BGR2GRAY);

        image_u8_t im = { .width = img_gray_.cols,
            .height = img_gray_.rows,
            .stride = img_gray_.cols,
            .buf = img_gray_.data
        };
        zarray_t *detections = apriltag_detector_detect(td_, &im);


        // Draw detection outlines
        for (int i = 0; i < zarray_size(detections); i++) {
            apriltag_detection_t *det;
            zarray_get(detections, i, &det);
            cv::line(img_, cv::Point(det->p[0][0], det->p[0][1]),
                        cv::Point(det->p[1][0], det->p[1][1]),
                        cv::Scalar(0, 0, 255), 2);
            cv::line(img_, cv::Point(det->p[0][0], det->p[0][1]),
                        cv::Point(det->p[3][0], det->p[3][1]),
                        cv::Scalar(0, 255, 0), 2);
            cv::line(img_, cv::Point(det->p[1][0], det->p[1][1]),
                        cv::Point(det->p[2][0], det->p[2][1]),
                        cv::Scalar(0xff, 0, 0), 2);
            cv::line(img_, cv::Point(det->p[2][0], det->p[2][1]),
                        cv::Point(det->p[3][0], det->p[3][1]),
                        cv::Scalar(0xff, 0, 0), 2);

            stringstream ss;
            ss << det->id;
            cv::String text = ss.str();
            int fontface = cv::FONT_HERSHEY_SCRIPT_SIMPLEX;
            double fontscale = 1.0;
            int baseline;
            cv::Size textsize = getTextSize(text, fontface, fontscale, 2,
                                            &baseline);
            putText(img_, text, cv::Point(det->c[0]-textsize.width/2,
                                        det->c[1]+textsize.height/2),
                    fontface, fontscale, cv::Scalar(0xff, 0x99, 0), 2);
            
            std::vector<cv::Point3d > standaloneTagObjectPoints;
            std::vector<cv::Point2d > standaloneTagImagePoints;
            addObjectPoints(tag_size_/2, cv::Matx44d::eye(), standaloneTagObjectPoints);
            addImagePoints(det, standaloneTagImagePoints);
            Eigen::Matrix4d transform = getRelativeTransform(standaloneTagObjectPoints,
                                                            standaloneTagImagePoints,
                                                            camera_matrix_.at<double>(0,0), camera_matrix_.at<double>(1,1), camera_matrix_.at<double>(0,2), camera_matrix_.at<double>(1,2));

            if(det->id==0){
                transform = T_camera_livox_.inverse()*transform;
                Eigen::Matrix3d rot = transform.block(0, 0, 3, 3);
                Eigen::Quaterniond rot_quaternion(rot);
                Eigen::Quaterniond q_temp(Eigen::AngleAxisd(-M_PI/2,Eigen::Vector3d::UnitX()));
                rot_quaternion = rot_quaternion*q_temp;
                q_temp = Eigen::Quaterniond(Eigen::AngleAxisd(M_PI/2,Eigen::Vector3d::UnitZ()));
                rot_quaternion = rot_quaternion*q_temp;
                transform.block(0,0,3,3) = rot_quaternion.toRotationMatrix();

                Eigen::Matrix3d rot_show = transform.block(0, 0, 3, 3);
                Eigen::Quaterniond rot_q_show(rot_show);

                //pub odom
                nav_msgs::Odometry tag_odom_cur_msg;
                tag_odom_cur_msg.header.frame_id = "livox_frame";
                tag_odom_cur_msg.child_frame_id = "tag0";
                tag_odom_cur_msg.header.stamp = ros::Time::now();
                geometry_msgs::Pose pose_msg_temp;
                pose_msg_temp.position.x = transform(0,3);
                pose_msg_temp.position.y = transform(1,3);
                pose_msg_temp.position.z = transform(2,3);
                pose_msg_temp.orientation.w = rot_q_show.w();
                pose_msg_temp.orientation.x = rot_q_show.x();
                pose_msg_temp.orientation.y = rot_q_show.y();
                pose_msg_temp.orientation.z = rot_q_show.z();
                tag_odom_cur_msg.pose.pose = pose_msg_temp;
                tag_odom_cur_msg.twist.twist.linear.x = 0;
                tag_odom_cur_msg.twist.twist.linear.y = 0;
                tag_odom_cur_msg.twist.twist.linear.z = 0;
                tag0_odom_pub_.publish(tag_odom_cur_msg);
            }
            if(det->id==2){
                transform = T_camera_livox_.inverse()*transform;
                Eigen::Matrix3d rot = transform.block(0, 0, 3, 3);
                Eigen::Quaterniond rot_quaternion(rot);
                Eigen::Quaterniond q_temp(Eigen::AngleAxisd(-M_PI/2,Eigen::Vector3d::UnitX()));
                rot_quaternion = rot_quaternion*q_temp;
                q_temp = Eigen::Quaterniond(Eigen::AngleAxisd(M_PI/2,Eigen::Vector3d::UnitZ()));
                rot_quaternion = rot_quaternion*q_temp;
                transform.block(0,0,3,3) = rot_quaternion.toRotationMatrix();

                Eigen::Matrix3d rot_show = transform.block(0, 0, 3, 3);
                Eigen::Quaterniond rot_q_show(rot_show);

                //pub odom
                nav_msgs::Odometry tag_odom_cur_msg;
                tag_odom_cur_msg.header.frame_id = "livox_frame";
                tag_odom_cur_msg.child_frame_id = "tag2";
                tag_odom_cur_msg.header.stamp = ros::Time::now();
                geometry_msgs::Pose pose_msg_temp;
                pose_msg_temp.position.x = transform(0,3);
                pose_msg_temp.position.y = transform(1,3);
                pose_msg_temp.position.z = transform(2,3);
                pose_msg_temp.orientation.w = rot_q_show.w();
                pose_msg_temp.orientation.x = rot_q_show.x();
                pose_msg_temp.orientation.y = rot_q_show.y();
                pose_msg_temp.orientation.z = rot_q_show.z();
                tag_odom_cur_msg.pose.pose = pose_msg_temp;
                tag_odom_cur_msg.twist.twist.linear.x = 0;
                tag_odom_cur_msg.twist.twist.linear.y = 0;
                tag_odom_cur_msg.twist.twist.linear.z = 0;
                tag2_odom_pub_.publish(tag_odom_cur_msg);
            }
        }
        apriltag_detections_destroy(detections);

        imshow("Tag Detections", img_);
        cv::waitKey(1);

        sensor_msgs::PointCloud2 pc_msg;
        pcl::toROSMsg(*pc_tmp, pc_msg);
        pc_msg.header.stamp = ros::Time::now();
        pc_msg.header.frame_id = "livox_frame";
        cloud_pub_.publish(pc_msg);


        string tranin_or_test="training";
        //   string tranin_or_test="testing";
        string dataset_name = "06042";
    //     cout<<tranin_or_test<<endl;
        cout<<frame_id_string_<<endl;
    //    pc_path_name_="/home/st/ubuntu_data/dataset/gazebo/"+dataset_name+"/"+tranin_or_test+"/label_2/"+frame_id_string_+".txt";
    //    saveLabel(pc_path_name_,label);


    pc_path_name_="/media/st/data_fast/keti/"+dataset_name+"/"+tranin_or_test+"/velodyne/"+frame_id_string_+".pcd";
    pcl::io::savePCDFile(pc_path_name_, *pc_tmp);

    cv::imwrite("/media/st/data_fast/keti/"+dataset_name+"/"+tranin_or_test+"/image_2/"+frame_id_string_+".png",img_);

    //    cv_bridge::CvImagePtr image_front;
    //    image_front = cv_bridge::toCvCopy(front_image_msg, sensor_msgs::image_encodings::BGR8);
    //    cv::imwrite("/home/st/ubuntu_data/dataset/gazebo/"+dataset_name+"/"+tranin_or_test+"/image_2/"+frame_id_string_+".png",(*image_front).image);
    //    string img_center_path_name_;
    //    img_center_path_name_="/home/st/ubuntu_data/dataset/gazebo/"+dataset_name+"/"+tranin_or_test+"/"+"img_center_label.txt";
    //     Eigen::Matrix4f T_cam_livox = Eigen::Matrix4f::Identity();
    //     temp_q = Eigen::Quaternionf(T_cam2lidar.getRotation().getW(),T_cam2lidar.getRotation().getX(),
    //                                                                     T_cam2lidar.getRotation().getY(),T_cam2lidar.getRotation().getZ());
    //     T_cam_livox.block<3, 3>(0, 0) = temp_q.toRotationMatrix();
    //     T_cam_livox.block<3, 1>(0, 3) = Eigen::Vector3f(T_cam2lidar.getOrigin().x(), T_cam2lidar.getOrigin().y(), T_cam2lidar.getOrigin().z());
    //     Eigen::Matrix4f T_livox_plane = T_plane_livox.inverse();
    //     Eigen::Matrix4f T_plane_c_plane = Eigen::Matrix4f::Identity();
    //     temp_q = Eigen::Quaternionf(T_c_plane_plane.inverse().getRotation().getW(),T_c_plane_plane.inverse().getRotation().getX(),
    //                                                                 T_c_plane_plane.inverse().getRotation().getY(),T_c_plane_plane.inverse().getRotation().getZ());
    //     T_plane_c_plane.block<3, 3>(0, 0) = temp_q.toRotationMatrix();
    //     T_plane_c_plane.block<3, 1>(0, 3) = Eigen::Vector3f(T_c_plane_plane.inverse().getOrigin().x(), T_c_plane_plane.inverse().getOrigin().y(), T_c_plane_plane.inverse().getOrigin().z());
    //     saveImgLabel(img_center_path_name_, label, "/home/st/ubuntu_data/dataset/gazebo/"+dataset_name+"/"+tranin_or_test+"/image_2/"+frame_id_string_+".png", 
    //                                    (*image_front).image , T_cam_livox, T_livox_plane, T_plane_c_plane);

        frame_num_++;

        ros::Duration(0.01).sleep();
    }
}

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "mcl_3dl");
    ros::NodeHandle nh;
    nh.param<double>("time_interval",time_interval_,1);
//    tf2_ros::TransformListener tfl_(tfbuf_);
//    frame_ids_["odom"]=std::string("odom");
//    frame_ids_["base_link"]=std::string("base_link");
//    frame_ids_["map"]=std::string("map");
   cloud_pub_=nh.advertise<sensor_msgs::PointCloud2>("pc_handle",10,true);

    tf_ = tag36h11_create();
    td_ = apriltag_detector_create();
    apriltag_detector_add_family(td_, tf_);
    td_->quad_decimate = 1.0;
    td_->quad_sigma = 0.0;
    td_->nthreads = 2;
    td_->debug = 0;
    td_->refine_edges = 1;

    T_camera_livox_<<0.000509857,-0.999954,-0.00956595,0.0128984,
                                        -0.0100791,0.00956033,-0.999904,-0.0734674,
                                        0.999949,0.000606224,-0.0100737,-0.0226611,
                                        0,0,0,1;

    // P2_ = Eigen::Matrix4f::Identity();
    // float p2[12] = {319.9988245765257, 0, 320.5, 0,
    //                              0, 319.9988245765257, 240.5, 0, 
    //                              0, 0, 1, 0};
    // for (int i = 0; i<12; i++){
    //     P2_(i/4, i%4) = p2[i];
    // }

    tag0_odom_pub_ = nh.advertise<nav_msgs::Odometry>("/tag0/odom", 1);
    tag2_odom_pub_ = nh.advertise<nav_msgs::Odometry>("/tag2/odom", 1);

    ros::Subscriber pc_sub =  nh.subscribe("/livox/lidar", 20000, pc_custom_CB, ros::TransportHints().tcpNoDelay());
    ros::Subscriber img_sub =  nh.subscribe("/camera/color/image_raw/compressed", 20000, img_CB, ros::TransportHints().tcpNoDelay());

    thread process_thread(processThread);
    process_thread.detach();

    cout<<"debug "<<endl;

    ros::MultiThreadedSpinner spinner(2);
    spinner.spin();

    return 0;
}
